#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/MarkerArray.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
#include <tf/transform_listener.h>
#include <sstream>
#include <mutex>     // 线程安全支持
#include <algorithm> // std::max

#include "graph.hpp"

class GlobalPlanner
{
public:
    GlobalPlanner()
    {
        ros::NodeHandle nh;
        ros::NodeHandle nhp("~");

        // 参数获取与校验
        nhp.getParam("gp_graph_topic", graph_topic_);
        nhp.getParam("gp_global_plan_topic", global_plan_topic_);
        nhp.getParam("gp_nav_goal_topic", nav_goal_topic_);
        nhp.getParam("gp_map_frame_id", map_frame_id_);
        nhp.getParam("gp_robot_frame_id", robot_frame_id_);
        nhp.getParam("gp_cancel_request_topic", cancel_request_topic_);
        nhp.getParam("gp_reset_request_topic", reset_request_topic_);
        nhp.getParam("gp_block_respond_topic", block_respond_topic_);

        // 频率参数校验（确保非负）
        nhp.getParam("gp_plan_publish_rate", plan_publish_rate_);

        // 订阅者初始化
        graph_sub_ = nh.subscribe(graph_topic_, 1, &GlobalPlanner::graphCallback, this);
        goal_sub_ = nh.subscribe(nav_goal_topic_, 1, &GlobalPlanner::planRequestCallback, this);
        cancel_sub_ = nh.subscribe(cancel_request_topic_, 1, &GlobalPlanner::cancelRequestCallback, this);
        reset_sub_ = nh.subscribe(reset_request_topic_, 1, &GlobalPlanner::resetRequestCallback, this);
        block_sub_ = nh.subscribe(block_respond_topic_, 1, &GlobalPlanner::blockRespondCallback, this);

        // 发布者初始化
        plan_pub_ = nh.advertise<nav_msgs::Path>(global_plan_topic_, 1);

        // 定时器初始化
        plan_timer_ = nh.createTimer(ros::Duration(1.0 / plan_publish_rate_),
                                     &GlobalPlanner::planPublishTimerCallback, this);

        planning_ = false;
        tf_listener_ = new tf::TransformListener();
    }

    ~GlobalPlanner()
    {
        delete tf_listener_;
    }

private:
    Graph graph_;
    Graph original_graph_;
    bool planning_;
    std::vector<Node> current_path_;
    std::mutex graph_mutex_; // 线程安全锁 [[7]]

    // 参数变量
    std::string graph_topic_;
    std::string global_plan_topic_;
    std::string nav_goal_topic_;
    std::string map_frame_id_;
    std::string robot_frame_id_;
    std::string cancel_request_topic_;
    std::string reset_request_topic_;
    std::string block_respond_topic_;
    double plan_publish_rate_;

    // ROS句柄
    ros::Subscriber graph_sub_;
    ros::Subscriber goal_sub_;
    ros::Subscriber cancel_sub_;
    ros::Subscriber reset_sub_;
    ros::Subscriber block_sub_;
    ros::Publisher plan_pub_;
    ros::Timer plan_timer_;
    tf::TransformListener *tf_listener_;

    void graphCallback(const visualization_msgs::MarkerArray::ConstPtr &msg)
    {
        std::lock_guard<std::mutex> lock(graph_mutex_);
        // ROS_INFO("Recv data from graph topic.");
        original_graph_.clear();
        for (const auto &marker : msg->markers)
            if (marker.type == visualization_msgs::Marker::POINTS)
                for (const auto &point : marker.points)
                    original_graph_.add_node(point.x, point.y, point.z);

        for (const auto &marker : msg->markers)
            if (marker.type == visualization_msgs::Marker::LINE_LIST)
                for (size_t i = 0; i < marker.points.size(); i += 2)
                {
                    size_t from_idx = original_graph_.find_nearest_node(marker.points[i].x, marker.points[i].y, marker.points[i].z);
                    size_t to_idx = original_graph_.find_nearest_node(marker.points[i + 1].x, marker.points[i + 1].y, marker.points[i + 1].z);

                    if (from_idx != to_idx)
                        original_graph_.add_edge(from_idx, to_idx);
                }

        // ROS_INFO("Graph loaded, number of origin_nodes: %ld, number of origin_edges: %ld" , original_graph_.num_nodes(), original_graph_.num_edges());
        // ROS_INFO("Graph check, number of nodes: %ld, number of edges: %ld" , graph_.num_nodes(), graph_.num_edges());
        if ((graph_.num_nodes() == 0 || graph_.num_edges() == 0) && (original_graph_.num_edges() != 0 && original_graph_.num_nodes() != 0))
        {
            graph_.copy_from(original_graph_);
            ROS_INFO("<global planner> Graph updated, %ld nodes and %ld edges loaded.", graph_.num_nodes(), graph_.num_edges());
        }
    }

    size_t findNearestNode(float x, float y, float z) const
    {
        if (graph_.num_nodes() == 0)
            return 0;
        return graph_.find_nearest_node(x, y, z);
    }
    std::vector<Node> interpolatePath(const std::vector<Node> &path, double max_distance)
    {
        std::vector<Node> interpolated_path;
        if (path.empty())
            return interpolated_path;

        interpolated_path.push_back(path[0]); // 添加起点

        for (size_t i = 1; i < path.size(); ++i)
        {
            const Node &prev = path[i - 1];
            const Node &curr = path[i];

            double dx = curr.x - prev.x;
            double dy = curr.y - prev.y;
            double dz = curr.z - prev.z;
            double distance = sqrt(dx * dx + dy * dy + dz * dz);

            if (distance <= max_distance)
            {
                interpolated_path.push_back(curr);
                continue;
            }

            int num_steps = static_cast<int>(ceil(distance / max_distance));
            double step_x = dx / num_steps;
            double step_y = dy / num_steps;
            double step_z = dz / num_steps;

            for (int step = 1; step <= num_steps; ++step)
            {
                Node node;
                node.x = prev.x + step * step_x;
                node.y = prev.y + step * step_y;
                node.z = prev.z + step * step_z;
                interpolated_path.push_back(node);
            }
        }

        return interpolated_path;
    }
   
    void planRequestCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
    {
        if (graph_.num_nodes() < 2)
        {
            ROS_INFO("<global planner> Do not have enough nodes to plan.");
            return;
        }

        // 获取机器人当前位置
        tf::StampedTransform transform;
        try
        {
            tf_listener_->lookupTransform(map_frame_id_, robot_frame_id_,
                                          ros::Time(0), transform);
        }
        catch (tf::TransformException ex)
        {
            ROS_WARN("%s", ex.what());
            return;
        }

        Node start_node;
        start_node.x = transform.getOrigin().x();
        start_node.y = transform.getOrigin().y();
        start_node.z = transform.getOrigin().z();

        Node goal_node;
        goal_node.x = msg->pose.position.x;
        goal_node.y = msg->pose.position.y;
        goal_node.z = msg->pose.position.z;

        ROS_INFO("<global planner> Navigation plan requested from %d,%d,%d to %d,%d,%d",
                 (int)start_node.x, (int)start_node.y, (int)start_node.z, (int)goal_node.x, (int)goal_node.y, (int)goal_node.z);

        // 查找最近节点
        size_t start_idx = findNearestNode(start_node.x, start_node.y, start_node.z);
        size_t goal_idx = findNearestNode(goal_node.x, goal_node.y, goal_node.z);
        ROS_INFO("<global planner> extract to run from node %ld to node %ld", start_idx, goal_idx);

        // 路径规划
        {
            std::lock_guard<std::mutex> lock(graph_mutex_);
            current_path_ = graph_.find_way(start_idx, goal_idx);
        }

        if (!current_path_.empty())
        {
            planning_ = true;
            current_path_ = interpolatePath(current_path_, 0.2);
            planPublishTimerCallback(ros::TimerEvent());
            ROS_INFO("<global planner> Global Plan Success, length of path: %lu", current_path_.size());
        }
        else
        {
            ROS_WARN("<global planner> unable to find a path!");
            planning_ = false;
        }
    }

    void planPublishTimerCallback(const ros::TimerEvent &)
    {
        if (!planning_ || current_path_.empty())
            return;

        // 发布路径
        nav_msgs::Path path_msg;
        path_msg.header.frame_id = map_frame_id_;
        path_msg.header.stamp = ros::Time::now();

        for (const auto &node : current_path_)
        {
            geometry_msgs::PoseStamped pose;
            pose.header = path_msg.header;
            pose.pose.position.x = node.x;
            pose.pose.position.y = node.y;
            pose.pose.position.z = node.z;
            pose.pose.orientation.w = 1.0;
            path_msg.poses.push_back(pose);
        }

        plan_pub_.publish(path_msg);
    }

    void cancelRequestCallback(const std_msgs::Empty::ConstPtr &)
    {
        planning_ = false;
        ROS_INFO("规划已取消");
    }

    void resetRequestCallback(const std_msgs::Empty::ConstPtr &)
    {
        std::lock_guard<std::mutex> lock(graph_mutex_); // 线程安全 [[7]]
        planning_ = false;
        graph_.copy_from(original_graph_);
        current_path_.clear();
        ROS_INFO("地图已重置");
    }

    void blockRespondCallback(const std_msgs::String::ConstPtr &msg)
    {
        std::istringstream iss(msg->data);
        std::string token;
        std::vector<double> coords;

        // 解析输入字符串为浮点数
        while (std::getline(iss, token, ' '))
        {
            try
            {
                coords.push_back(std::stod(token)); // 使用 std::stod 转换为空格分隔的浮点数 [[5]][[9]]
            }
            catch (const std::invalid_argument &)
            {
                ROS_WARN("无效的坐标输入: %s", token.c_str());
                return;
            }
        }

        // 检查是否包含六个浮点数
        if (coords.size() != 6)
        {
            ROS_WARN("输入应包含六个浮点数，当前数量: %lu", coords.size());
            return;
        }

        // 提取两个三维坐标
        float from_x = static_cast<float>(coords[0]);
        float from_y = static_cast<float>(coords[1]);
        float from_z = static_cast<float>(coords[2]);
        float to_x = static_cast<float>(coords[3]);
        float to_y = static_cast<float>(coords[4]);
        float to_z = static_cast<float>(coords[5]);
        Node from_node(from_x, from_y, from_z);
        Node to_node(to_x, to_y, to_z);

        size_t from_idx = graph_.find_nearest_node(from_x, from_y, from_z);
        size_t to_idx = graph_.find_nearest_node(to_x, to_y, to_z);

        std::lock_guard<std::mutex> lock(graph_mutex_); // 线程安全 [[5]]

        // 删除边
        if (graph_.delete_edge(
                from_idx,
                to_idx))
        {
            ROS_INFO("已删除阻塞边");

            // 重新规划路径（可选）
            if (planning_ && !current_path_.empty())
            {
                // 重新获取当前机器人位置作为起点 [[5]]
                tf::StampedTransform transform;
                try
                {
                    tf_listener_->lookupTransform(map_frame_id_, robot_frame_id_,
                                                  ros::Time(0), transform);
                }
                catch (tf::TransformException ex)
                {
                    ROS_WARN("%s", ex.what());
                    return;
                }

                Node start_node;
                start_node.x = transform.getOrigin().x();
                start_node.y = transform.getOrigin().y();
                start_node.z = 0.0;

                size_t start_idx = findNearestNode(start_node.x, start_node.y, start_node.z);
                size_t goal_idx = findNearestNode(current_path_.back().x, current_path_.back().y, current_path_.back().z);

                current_path_ = graph_.find_way(start_idx, goal_idx);
                current_path_ = interpolatePath(current_path_, 0.2);
                if (current_path_.empty())
                {
                    planning_ = false;
                    ROS_WARN("重新规划失败");
                }
            }
        }
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "global_navigation");
    GlobalPlanner planner;
    ros::spin();
    return 0;
}